
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_ahrs.c
  * @author     baiyang
  * @date       2021-8-23
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       是否稳定定位
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
bool fms_position_ok()
{
    return ahrs_valid_lpos(fms.ahrs);
}

/**
  * @brief       ekf是否有相对定位
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
bool fms_ekf_has_relative_position()
{
    return false;
}

/**
  * @brief       高度是否可用
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
bool fms_ekf_alt_ok()
{
    return ahrs_valid_alt(fms.ahrs);
}

// update_auto_armed - update status of auto_armed flag
void fms_update_auto_armed()
{
    // disarm checks
    if(fms.ap.auto_armed){
        // if motors are disarmed, auto_armed should also be false
        if(!fms.motors->_armed) {
            fms_set_auto_armed(false);
            return;
        }
        // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
        if(mode_has_manual_throttle(fms.flightmode) && fms.ap.throttle_zero && !fms.failsafe.radio) {
            fms_set_auto_armed(false);
        }

    }else{
        // arm checks
        
        // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
        if(fms.motors->_armed && fms.ap.using_interlock) {
            if(!fms.ap.throttle_zero && fms.motors->_spool_state == MB_MOTOR_THROTTLE_UNLIMITED) {
                fms_set_auto_armed(true);
            }
        // if motors are armed and throttle is above zero auto_armed should be true
        // if motors are armed and we are in throw mode, then auto_armed should be true
        } else if (fms.motors->_armed && !fms.ap.using_interlock) {
            if(!fms.ap.throttle_zero || mode_number(fms.flightmode) == THROW) {
                fms_set_auto_armed(true);
            }
        }
    }
}

// rotate vector from vehicle's perspective to North-East frame
void fms_rotate_body_frame_to_NE(float *x, float *y)
{
    float ne_x = (*x) * fms.ahrs->cos_yaw - (*y) * fms.ahrs->sin_yaw;
    float ne_y = (*x) * fms.ahrs->sin_yaw + (*y) * fms.ahrs->cos_yaw;
    *x = ne_x;
    *y = ne_y;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_update_ahrs()
{
    ahrs_view_update();

    fms_update_home_from_EKF();

    // pull position from ahrs
    Location loc;
    ahrs_get_location(fms.ahrs, &loc);
    fms.current_loc.lat = loc.lat;
    fms.current_loc.lng = loc.lng;

    // exit immediately if we do not have an altitude estimate
    if (!fms.ahrs->valid_alt) {
        return;
    }

    // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
    const int32_t alt_above_origin_cm = ahrs_get_position_z_up_cm(fms.ahrs);
    location_set_alt_cm(&fms.current_loc, alt_above_origin_cm, ALT_FRAME_ABOVE_ORIGIN);

    if (!ahrs_home_is_set(fms.ahrs) || !location_change_alt_frame(&fms.current_loc, ALT_FRAME_ABOVE_HOME)) {
        // if home has not been set yet we treat alt-above-origin as alt-above-home
        location_set_alt_cm(&fms.current_loc, alt_above_origin_cm, ALT_FRAME_ABOVE_HOME);
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_ahrs_init()
{
    ahrs_view_init();
}

// check for high vibrations affecting altitude control
void fms_check_vibration()
{
    uint32_t now = time_millis();

    uitc_estimator_innovations innovations;
    uitc_estimator_status status;

    // assume checks will succeed
    bool innovation_checks_valid = true;

    // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
    if (itc_copy_from_hub(ITC_ID(estimator_innovations), &innovations) != 0) {
        innovation_checks_valid = false;
    }
    const bool innov_velD_posD_positive = math_flt_positive((innovations.ev_vvel + innovations.gps_vvel + innovations.aux_vvel) / 3)
                                          && math_flt_positive((innovations.baro_vpos + innovations.ev_vpos + innovations.gps_vpos) / 3);

    // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0)
    if (itc_copy_from_hub(ITC_ID(estimator_status), &status) != 0) {
        innovation_checks_valid = false;
    }

    const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && !isnan(status.vel_test_ratio) && (status.vel_test_ratio > 1.0f));
    const bool do_bad_vibe_actions = (fms.g.fs_vibe_enabled == 1) && bad_vibe_detected && fms.motors->_armed && !mode_has_manual_throttle(fms.flightmode);

    if (!fms.vibration_check.high_vibes) {
        // initialise timers
        if (!do_bad_vibe_actions) {
            fms.vibration_check.start_ms = now;
        }
        // check if failure has persisted for at least 1 second
        if (now - fms.vibration_check.start_ms > 1000) {
            // switch position controller to use resistant gains
            fms.vibration_check.clear_ms = 0;
            fms.vibration_check.high_vibes = true;
            posctrl_set_vibe_comp(fms.pos_control, true);
            //AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
            gcs_send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
        }
    } else {
        // initialise timer
        if (do_bad_vibe_actions) {
            fms.vibration_check.clear_ms = now;
        }
        // turn off vibration compensation after 15 seconds
        if (now - fms.vibration_check.clear_ms > 15000) {
            // restore position controller gains, reset timers and update user
            fms.vibration_check.start_ms = 0;
            fms.vibration_check.high_vibes = false;
            posctrl_set_vibe_comp(fms.pos_control, false);
            fms.vibration_check.clear_ms = 0;
            //AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
            gcs_send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
        }
    }

    return;
}

/*------------------------------------test------------------------------------*/


